#pragma once
#include <iostream>
#include <vector>
#include <cmath>
namespace common_lib
{
    struct Point
    {
        float x, y;
        Point()
        {
            x = 0.0f;
            y = 0.0f;
        }
    };
    class Common
    {
    public:
        void SplitString(const std::string &s, std::vector<std::string> &v, const std::string &c);
        float AngleConstraintToPI(float angle) const;
        double ValueConstraintMinToMax(double value, double min, double max);

        float Degree2Radian(float degree_value);
        float Radian2Degree(float radian_value);
        float Point2LineDistanceWithSign(float p_x, float p_y, float l_x, float l_y, float l_theta);
        bool  Point2LineProjection(float p_x, float p_y, float begin_x, float begin_y, float end_x, float end_y, float &pj_x, float &pj_y);
        bool  PointInLineSegment(float p_x, float p_y, float begin_x, float begin_y, float end_x, float end_y);
        float SectionLinearFcn(std::map<float,float> point,float x);
        double getVectorAngle2d(const double angle, const float x, const float y);
        double getVectorAngle2d(const double x1, const double y1, const double x2, const double y2);
    };

    enum DoubleThreshold_e
    {
        UP_AREA = 0,
        DOEN_AREA
    };
    class DoubleThreshold
    {
    private:
        float down_threshold_;
        float up_threshold_;
        DoubleThreshold_e last_state_;
    public:
        DoubleThreshold(float down_threshold, float up_threshold) : down_threshold_(down_threshold), up_threshold_(up_threshold){last_state_ = DoubleThreshold_e::DOEN_AREA;};
        //~DoubleThreshold();
        DoubleThreshold_e GetState(float value);
        void SetDownThreshold(const float & down_threshold);
        void SetUpThreshold(const float & up_threshold);
    };

    class TON
    {
    private:
        bool is_first_;
        double start_time_;
    public:
        TON();
        bool Q(bool state, float delay_time);
    };
    
    class TOF
    {
    private:
        bool is_first_;
        double start_time_;

    public:
        TOF();
        bool Q(bool state, float delay_time);
    };

    class SpeedSmoothBase
    {
    public:
        float last_output_;
        float period_s_;
        float target_;
        float output_;
        virtual ~SpeedSmoothBase() {};

        virtual float VelocityConfig(float target_speed, float current_speed, float acc, float dcc) = 0;
        virtual float VelocityConfigToEnd(float end_dis, float current_speed, float acc, float dcc) = 0;
        virtual void Reset() = 0;
    };

    class SpeedSmoothWithCloseConstAcc : public SpeedSmoothBase
    {
    private:
        float last_output_;
        float period_s_;
        float target_;
        float output_;
    public:
        SpeedSmoothWithCloseConstAcc(float period_s);
        ~SpeedSmoothWithCloseConstAcc();

    public:
        float VelocityConfig(float target_speed, float current_speed, float acc, float dcc) override;
        float VelocityConfigToEnd(float end_dis, float current_speed, float acc, float dcc) override;
        void Reset() override;
    };
};